#include "mpu6050_task.h"
#include "cmsis_os.h"
#include "math.h"

mpu6050_t mpu6050;
extern mpu6050_data_t mpu6050_data;

void mpu6050_task(void const *pvParameters)
{
    while (1)
    {
        mpu6050.GetInfo();
    }
    

}


void mpu6050_t::GetInfo()
{
    mpu6050_read_accel();
    mpu6050_read_gyro();
    mpu6050_read_temp();
    mpu6050.accel_x = mpu6050_data.accel_x_data;
    mpu6050.accel_y = mpu6050_data.accel_y_data;
    mpu6050.accel_z = mpu6050_data.accel_z_data;
    mpu6050.gyro_x = mpu6050_data.gyro_x_data;
    mpu6050.gyro_y = mpu6050_data.gyro_y_data;
    mpu6050.gyro_z = mpu6050_data.gyro_z_data;
    mpu6050.temp = mpu6050_data.temp_data;
}

void mpu6050_t::Calc()
{
    accel_pitch = atan2(accel_y,sqrt(accel_x*accel_x+accel_z*accel_z));
    accel_roll = atan2(-accel_x, sqrt(accel_y*accel_y + accel_z*accel_z));

    static float angle_pitch = 0, angle_roll = 0;
    angle_pitch = ALPHA * (angle_pitch + gyro_y * DT) + (1-ALPHA) * accel_pitch;
    angle_roll  = ALPHA * (angle_roll  + gyro_x * DT) + (1-ALPHA) * accel_roll;

    pitch = angle_pitch*180.0f/PI;
    roll = angle_roll*180.0f/PI;
}

